#include "DISTANCE_SENSOR_module/DISTANCE_SENSOR.hpp"

#ifdef DBG_TAG
#undef DBG_TAG
#endif
#ifdef DBG_LVL
#undef DBG_LVL
#endif

#define DBG_TAG     "distance"
#define DBG_LVL     DBG_LOG

extern "C"
{
#include <rtdbg.h>
}

using namespace std;

/******************************************************************************
 *
 * @brief:     DISTANCE_SONIC类实现
 *
 ******************************************************************************/

DISTANCE_SONIC::DISTANCE_SONIC(CHIP_UART *uart,CHIP_OUTPUT_PIN *rs485_en)
{
    if (!uart || !rs485_en)
    {
        LOG_E("input para is NULL");
        goto end;
    }

    if (uart->CHIP_UART_read_init_status() != chip_uart_status_ok) goto end;
    if (rs485_en->CHIP_OUTPUT_read_init_status() != chip_pin_status_ok) goto end;

    this->uart = uart;
    this->rs485_en = rs485_en;
    debug_flag = 0;

    DISTANCE_SONIC_change_angle();

    init_flag = distance_status_ok;
    LOG_I("sonic create ok");
    return;
    end: init_flag = distance_status_fail;
    LOG_E("sonic create fail");
    return;

}

DISTANCE_SONIC::~DISTANCE_SONIC()
{

}

distance_status DISTANCE_SONIC::DISTANCE_SONIC_read_distance(double *read_buff)
{
    chip_uart_message * msg = NULL;
    uint32_t distance;
    double temp;
    uint8_t buff2[]={0x01,0x03,0x01,0x00,0x00,0x01,0x85,0xF6};//读取处理距离值
    if(init_flag != distance_status_ok)
        goto end;

    if(!read_buff)
    {
        LOG_E("cant read for null");
        goto end;
    }

    rs485_en->CHIP_OUTPUT_PIN_write(chip_pin_level_HIGH);
    this->uart->CHIP_UART_send(8, buff2);
    rs485_en->CHIP_OUTPUT_PIN_write(chip_pin_level_LOW);
    msg = uart->CHIP_UART_read_receive(1000);
    if(msg)
    {
//            LOG_D("recevied!");
//            for(i = 0;i < msg->now_end_pos;i++)
//            {
//                LOG_D("<%d>:<0x%.2X>",i,msg->message[i]);
//            }
        distance = 0;
        distance |= (msg->message[3]<<8);
        distance |= (msg->message[4]);

        delete msg;
        msg = NULL;

        temp = (double)distance;
        if(temp > 10000 )
        {
            LOG_E("error sonic value <%f>",temp);
            goto end;
        }
        *read_buff = temp;
        if(debug_flag)
            LOG_D("distance <%f>",*read_buff);
    }
    else
    {
//        LOG_E("no sonic recevied!");
        goto end;
    }


    return distance_status_ok;
    end:
    return distance_status_fail;
}

distance_status DISTANCE_SONIC::DISTANCE_SONIC_change_angle()
{
    uint8_t flag = 1;
    chip_uart_message * msg = nullptr;
    uint8_t buff1[]={0x01,0x06,0x02,0x08,0x00,0x01,0xC8,0x70};//XIUGAI CHULIJIAODU C870 0x01 0x06 0x02 0x08 0x00 0x01

    if(init_flag != distance_status_ok)
        goto end;

    rs485_en->CHIP_OUTPUT_PIN_write(chip_pin_level_HIGH);
    this->uart->CHIP_UART_send(8, buff1);
    rs485_en->CHIP_OUTPUT_PIN_write(chip_pin_level_LOW);

    while (flag)
    {
        msg = uart->CHIP_UART_read_receive(1000);
        if(msg)
        {
            delete msg;
            msg = nullptr;
            flag = 1;
        }
        else {
            flag = 0;
        }

    }

    LOG_I("set angle ok");
    return distance_status_ok;
    end:
    LOG_E("set angle fail");
    return distance_status_fail;
}

void DISTANCE_SONIC::DISTANCE_SONIC_open_debug()
{
    debug_flag = 1;
}

